from maix import touchscreen, camera, display, app, image, uart, time

disp = display.Display()
ts = touchscreen.TouchScreen()
# cam = camera.Camera(320, 240)
cam = camera.Camera(240, 180)

img = image.Image(240, 180)
devices = uart.list_devices()
serial = uart.UART(devices[0], 115200)
thresholds = [[0, 30, -10, 10, -10, 10]]    # black
# thresholds = [[0, 80, -120, -10, 0, 30]]    # green
# thresholds = [[0, 80, 30, 100, -120, -60]]  # blue
t = time.time_s()
max_ID=[-1,-1]
max_ID_up=[-1,-1]
y_target=85
deviation=0
XL=0
XR=0
YU=0
YD=0
UP_YU=0
UP_YD=0
range_stop=700
time0=0
time0last=0
laps=0
last_detection = 0

detect=True
mode=0
count=0

def is_in_button(x, y, btn_pos):
    return x > btn_pos[0] and x < btn_pos[0] + btn_pos[2] and y > btn_pos[1] and y < btn_pos[1] + btn_pos[3]
def the_exit_button():
    exit_label = "Exit>"
    size = image.string_size(exit_label)
    exit_btn_pos = [220, 0, 5*2 + size.width(), 5 * 2 + size.height()] 
    # draw exit button
    img.draw_string(190, 8, exit_label, image.COLOR_WHITE)
    # 图像按键坐标映射到屏幕上的坐标
    exit_btn_disp_pos = image.resize_map_pos(img.width(), img.height(), disp.width(), disp.height(), image.Fit.FIT_CONTAIN, exit_btn_pos[0], exit_btn_pos[1], exit_btn_pos[2], exit_btn_pos[3])
    x, y, pressed = ts.read()
    if is_in_button(x, y, exit_btn_disp_pos):
        app.set_exit_flag(True)

def set_is_in_button(x, y, btn_pos):
    return x > btn_pos[0] and x < btn_pos[0] + btn_pos[2] and y > btn_pos[1] and y < btn_pos[1] + btn_pos[3]

def the_set_button():
    global count, updated_touch
    global x, y, pressed
    set_label = "Set>"
    size = image.string_size(set_label)
    set_btn_pos = [220, 160, 5*2 + size.width(), 5 * 2 + size.height()] 
    # draw exit button
    img.draw_string(190, 158, set_label, image.COLOR_WHITE)
    # 图像按键坐标映射到屏幕上的坐标
    set_btn_disp_pos = image.resize_map_pos(img.width(), img.height(), disp.width(), disp.height(), image.Fit.FIT_CONTAIN, set_btn_pos[0], set_btn_pos[1], set_btn_pos[2], set_btn_pos[3])
    x, y, pressed = ts.read()
    # if set_is_in_button(x, y, set_btn_disp_pos):
    #     count = 0
    if pressed == 1 and set_is_in_button(x, y, set_btn_disp_pos):
        while pressed == 1:
            x, y, pressed = ts.read()
        count = 0



def find_max(blobs):
    max_size=[0,0]
    max_ID=[-1,-1]
    for i in range(len(blobs)):
        if blobs[i].pixels()>max_size[0]:
            max_ID[1]=max_ID[0]
            max_size[1]=max_size[0]
            max_ID[0]=i
            max_size[0]=blobs[i].pixels()
        elif blobs[i].pixels()>max_size[1]:
            max_ID[1]=i
            max_size[1]=blobs[i].pixels()
    return max_ID

cam.skip_frames(30)           # 跳过开头的30帧
while not app.need_exit():
    receive = serial.read()
    XL=120
    XR=120
    time0=time.time_s()-t
    img = cam.read()
    img = img.lens_corr(strength=1.8, zoom=1.0) #矫正畸变
    img.draw_string(200, 30, str(laps), image.COLOR_WHITE)
    img.draw_string(200, 52, str(time0), image.COLOR_WHITE)
    img.draw_string(200, 74, str(count), image.COLOR_WHITE)
    img.draw_string(140, 74, "count", image.COLOR_WHITE)
    img.draw_string(200, 96, str(mode), image.COLOR_WHITE)
    img.draw_string(140, 96, "mode", image.COLOR_WHITE)
    # blobs = img.find_blobs(thresholds,roi=[100, 110, 30, 80], pixels_threshold=1,merge=True)
    blobs_up = img.find_blobs(thresholds,roi=[100, 0, 35, 35], pixels_threshold=1,merge=True)
    # blobs_up_L = img.find_blobs(thresholds,roi=[65, 0, 35, 35], pixels_threshold=1,merge=True)
    # blobs_up_R = img.find_blobs(thresholds,roi=[135, 0, 35, 35], pixels_threshold=1,merge=True)
    blobs = img.find_blobs(thresholds,roi=[100, 0, 30, 180], pixels_threshold=1,merge=True)
    img.draw_rect(100, 0, 30, 180, image.COLOR_WHITE)

    img.draw_rect(100, 0, 35, 35, image.COLOR_RED,thickness=2)#center
    img.draw_rect(65, 0, 35, 35, image.COLOR_RED,thickness=2)
    img.draw_rect(135, 0, 35, 35, image.COLOR_RED,thickness=2)
    if blobs:
        if detect:
            if blobs_up:
                max_ID_up=[-1,-1]
                max_ID_up=find_max(blobs_up)
                img.draw_string(10, 74, str(blobs_up[max_ID_up[0]].pixels()), image.COLOR_WHITE)
                if blobs_up[max_ID_up[0]].pixels()>820:
                    UP_YU=blobs_up[max_ID_up[0]].cy() 
                    # print("交点坐标", UP_YU) 
                    img.draw_string(10, 52, str(UP_YU), image.COLOR_WHITE)
                    if max_ID_up[1]!=-1:
                        if(blobs_up[max_ID_up[0]].cy()>blobs_up[max_ID_up[1]].cy()):
                            UP_YU=blobs_up[max_ID_up[1]].cy()
                            UP_YD=blobs_up[max_ID_up[0]].cy()
                        else:
                            UP_YU=blobs_up[max_ID_up[0]].cy()
                            UP_YD=blobs_up[max_ID_up[1]].cy()                  
                    img.draw_rect(blobs_up[max_ID_up[0]][0],blobs_up[max_ID_up[0]][1],blobs_up[max_ID_up[0]][2], blobs_up[max_ID_up[0]][3], image.COLOR_RED,thickness=1)
                    img.draw_cross(blobs_up[max_ID_up[0]].cx(),blobs_up[max_ID_up[0]].cy(),image.COLOR_RED,thickness=2)
                    count += 1
                    # print(count)
                    last_detection = time.ticks_ms()  # 更新最后一次识别时间
                    # print(last_detection)
                    detect = False
        else:
            if time.ticks_ms() - last_detection > 100:
                detect = True

        max_ID=[-1,-1]
        max_ID=find_max(blobs)  #找最大色块
        # img.draw_string(10, 8, str(YU), image.COLOR_WHITE)
        img.draw_string(10, 30, str(blobs[max_ID[0]].pixels()), image.COLOR_WHITE)
        if blobs[max_ID[0]].pixels()<910:       
            img.draw_rect(blobs[max_ID[0]][0],blobs[max_ID[0]][1],blobs[max_ID[0]][2], blobs[max_ID[0]][3], image.COLOR_GREEN,thickness=2)
            img.draw_cross(blobs[max_ID[0]].cx(),blobs[max_ID[0]].cy(),image.COLOR_GREEN,thickness=2)
            # XL=blobs[max_ID[0]].cx()
            # XR=XL
            YU=blobs[max_ID[0]].cy()  
            #YU在这下面处理
            deviation = y_target - YU
            # print("循迹坐标", YU, deviation) 
            img.draw_string(10, 8, str(YU), image.COLOR_WHITE)         
            if max_ID[1]!=-1:
                if(blobs[max_ID[0]].cy()>blobs[max_ID[1]].cy()):
                    YU=blobs[max_ID[1]].cy()
                    YD=blobs[max_ID[0]].cy()
                else:
                    YU=blobs[max_ID[0]].cy()
                    YD=blobs[max_ID[1]].cy()                
            else:
                if (blobs[max_ID[0]].pixels()>range_stop and time0 - time0last >= 7):
                    laps=laps+1

                    time0last=time0

        if count == 3:
            # print("ok")
            mode = 1
            count = 0
    
        if receive == b'1':
            count = 0
            mode = 0

        print("receive: {}, mode: {}, count: {}".format(receive, mode, count))

    the_exit_button()
    the_set_button()

    data = str(YU) + ' ' + str(mode) + ' ' + "\r\n"
    # print(data)
    serial.write_str(data)
    disp.show(img)



